/**
 * @file:          StepController.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.07.20
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.07.20,19:49:35
 */

/* Include Files **************************************************************/
#include "StepController.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
void StepController_Init(struct StepController *self)
{
    self->Run = StepController_Run;

    self->step_accumulate = 0;
    self->pulse_count_last = 0;
    self->pulse_count = 0;
    self->UpdateParam = StepController_UpdateParam;
    self->SetInitStepFromRad = StepController_SetInitStepFromRad;

    self->UpdateParam(self);
}

float32 StepController_Run(struct StepController *self)
{
    if (self->en_pin == 0)
    {
        /* step_increment processing */
        self->pulse_count_last = self->pulse_count;
        self->pulse_count = self->mcu->GetInputPulseCounter();

        self->step_increment = self->pulse_count - self->pulse_count_last;
        if (self->step_increment < -(int32_t)(self->param_pulse_count_max >> 1))
        {
            self->step_increment += (int32)self->param_pulse_count_max;
        }
        else if (self->step_increment > (int32_t)(self->param_pulse_count_max >> 1))
        {
            self->step_increment -= (int32)self->param_pulse_count_max;
        }

        /* pulse trim */
        if (self->param_pulse_trim_en)
        {
            if (self->dir_change == 2)
            {
                self->dir_change = 1;
                if (self->dir_pin != 0)
                {
                    self->pulse_trim = -self->param_extra_trim_pulse +
                                       (self->pulse_on_dir_change - self->pulse_count);
                }
                else
                {
                    self->pulse_trim = self->param_extra_trim_pulse +
                                       (self->pulse_on_dir_change - self->pulse_count);
                }

                self->step_increment += self->pulse_trim;
                // self->mcu->AddInputPulseCounter(self->pulse_trim);
                self->trim_action_count = 0;
            }
            else if (self->dir_change == 1)
            {
                self->trim_action_count++;
                if (self->trim_action_count >= self->param_trim_action_period)
                {
                    self->dir_change = 0;
                    self->step_increment -= self->pulse_trim;
                    // self->mcu->AddInputPulseCounter(-self->pulse_trim);
                }
            }
        }

        self->step_accumulate += self->step_increment;

        self->step = (self->step + self->step_increment);

        if (self->param_single_turn_mode != 0)
        {
            if (self->step >= (int32)(self->steps_per_turn))
            {
                self->step_turns += self->step / (int32)self->steps_per_turn;
                self->step = self->step % self->steps_per_turn;
            }
            else if (self->step < (int32)0)
            {
                int32 tmp = 1 + abs_bm(self->step) / (int32)self->steps_per_turn;
                self->step_turns -= tmp;
                self->step += (int32)(tmp * (int32)self->steps_per_turn);
            }
        }
    }
    self->out_pulse_cmd = self->step_accumulate;
    // self->out_pos_cmd = self->step_accumulate * self->step2rad;

    return self->out_pos_cmd;
}

void StepController_UpdateParam(struct StepController *self)
{
    self->steps_per_turn = 50 * 4 * self->param_subdivision;
    self->step2rad = BM_2PI / self->steps_per_turn;
}

void StepController_SetInitStepFromRad(struct StepController *self, float rad)
{
    self->step = (int32)(rad / self->step2rad);
    self->step_turns = 0;
    self->step_accumulate = self->step;
}